/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "Emm_V5.h"
#include "arm.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* ������������� */
uint8_t motor_num=1; 
bool control_motor;
										 
#define FRAME_HEADER 0xAA
#define FRAME_FOOTER 0x55
#define BUFFER_SIZE 32

uint8_t rx_buffer[BUFFER_SIZE];
uint8_t rx_index = 0;
uint8_t data;									 


/* ������������� */
#define ARM_FRAME_HEADER 0xBB  // 机械臂指令帧头
#define ARM_FRAME_FOOTER 0x66  // 机械臂指令帧尾
//#define ARM_BUFFER_SIZE 32
uint8_t uart_rx_buffer[BUFFER_SIZE];  
uint8_t rx_buffer_index = 0;          
uint8_t received_byte; 
//#define MAX_JOINTS 4           // 机械臂关节数量

uint8_t duoji_flag;
uint8_t duoji_flag_done = 0;
uint32_t jiaodu2,jiaodu3,jiaodu4;
uint32_t duoji_wait;
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void processCommand(char* cmd, uint8_t length)
{   
//	  printf("in");
    int16_t Vx, Vy, Vz;
    if(sscanf(cmd, "%hd,%hd,%hd", &Vx, &Vy, &Vz) == 3)
    {   
//        printf("Received: %hd,%hd,%hd\n", Vx, Vy, Vz);    
        Move_Transfrom(Vx, Vy, Vz);
        control_motor = 1;  
    }
    else
    {
//        printf("Invalid command format\n");
    }
}

void process_arm_command(char* cmd1, uint8_t length1)
{   
    int16_t duoji2, duoji3, duoji4,duoji5;
    if(sscanf(cmd1, "%hd,%hd,%hd,%hd", &duoji2, &duoji3, &duoji4, &duoji5) == 4)
    {  
				if(duoji2 >= 0 && duoji2 <= 180 && duoji3 >= 0 && duoji3 <= 180 && duoji4 >= 0 && duoji4 <= 95 && duoji5 >= 0 && duoji5 <= 180)
				{
       SET_B(0,duoji2, duoji3, duoji4,duoji5,0);
				}					
         
    }
}
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM5_Init();
  MX_TIM6_Init();
  MX_TIM7_Init();
  MX_UART4_Init();
  MX_UART5_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */
		for(uint8_t i=1;i<=4;i++)
	{
		Emm_V5_En_Control(i,1,0);
		HAL_Delay(10);
	}
	HAL_Delay(50);
	

	
	HAL_TIM_Base_Start_IT(&htim1);//底盘电机
	HAL_TIM_Base_Start_IT(&htim5);
	HAL_TIM_Base_Start_IT(&htim7);//

	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
	
	// duoji° down is +   up is -   舵机往下是加度数 往上是减度数    0~180°
  DUO_JI_Init(&B.duoji1,&htim2,TIM_CHANNEL_1,10); 	      //NO.1 jiazhua       最右边     度数大是open  度数小是close
	DUO_JI_Init(&B.duoji2,&htim2,TIM_CHANNEL_2,150);       
	DUO_JI_Init(&B.duoji3,&htim2,TIM_CHANNEL_3,90);          // NO.234 jixiebi  从上往下
	DUO_JI_Init(&B.duoji4,&htim2,TIM_CHANNEL_4,0);            // 0~90° !!!
  DUO_JI_Init(&B.duoji5,&htim4,TIM_CHANNEL_2,90);	        // yuntai   NO.5     左边倒二    顺时针shun——   逆时针ni＋
	duoji_flag = 1; //启动舵机缓速
	HAL_UART_Receive_IT(&huart4,&data,1); 
	HAL_UART_Receive_IT(&huart3,&data,1); 
//	printf("ok");
//	 HAL_Delay(1000);
//	 SET_B(5,90,0,0,0,0);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
		
//		Control_Servo_1(30);
//		if(data)
//		{
//		printf("%d",rx_buffer[0]);
//		printf("%d",rx_buffer[1]);
//		printf("%d",rx_buffer[2]);
//		}
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)   
{
	if(htim==&htim1)//
	{
		if(control_motor)  //
		{
			if(motor_num==1)			motor_speed(motor_num,-TargetB);
			else if(motor_num==2)	motor_speed(motor_num,TargetC);
			else if(motor_num==3)	motor_speed(motor_num,TargetD);
			else if(motor_num==4)	motor_speed(motor_num,-TargetA);
			motor_num++;
			if(motor_num==5)
			{
				motor_num=1;control_motor=0;
			}
		}
	}
		if(htim==&htim7)//10
	{ 
		if(duoji_flag!=0)
		{
			follow(&B.duoji1.now_follow,&B.duoji1.control_target,8,8);
			follow(&B.duoji2.now_follow,&B.duoji2.control_target,8,8);         // 8->15
			follow(&B.duoji3.now_follow,&B.duoji3.control_target,8,8);
			follow(&B.duoji4.now_follow,&B.duoji4.control_target,8,8);
			follow(&B.duoji5.now_follow,&B.duoji5.control_target,8,8);
			B.duoji1.pwm_=(int32_t)map(B.duoji1.now_follow,0,180,50,250);//
			B.duoji2.pwm_=(int32_t)map(B.duoji2.now_follow,0,180,50,250);//占空比
			B.duoji3.pwm_=(int32_t)map(B.duoji3.now_follow,0,180,50,250);
			B.duoji4.pwm_=(int32_t)map(B.duoji4.now_follow,0,180,50,250);
			B.duoji5.pwm_=(int32_t)map(B.duoji5.now_follow,0,180,50,250);
			DUO_JI_Control(&B.duoji1);
			DUO_JI_Control(&B.duoji2);
			DUO_JI_Control(&B.duoji3);
			DUO_JI_Control(&B.duoji4);
			DUO_JI_Control(&B.duoji5);
		}
	}
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart->Instance == UART4)
   {    
        static bool inFrame = false;  // 标记是否处于一帧数据接收过程中
        if(data ==  FRAME_HEADER)  // 检测到帧头
        {
            rx_index = 0;  // 清空缓冲区索引
            inFrame = true;  // 进入数据帧接收状态
					rx_buffer[rx_index++]=data;
        }
        else if(inFrame)
        {
            if(data == FRAME_FOOTER)  // 检测到帧尾
            {   rx_buffer[rx_index++] = data;  //存储帧尾
                rx_buffer[rx_index] = '\0';    //添加字符结束符
                processCommand((char*)rx_buffer+1,rx_index-2); //跳过帧头帧尾  
                inFrame = false;  // 结束数据帧接收
                rx_index = 0;  // 重置索引
            }
            else if(rx_index<BUFFER_SIZE-1)
            {
                rx_buffer[rx_index++] = data;  
            }
						else
						{
							inFrame = false;
							rx_index=0;
							
								
						}
        }
        HAL_UART_Receive_IT(&huart4, &data, 1);
    }
	 if(huart->Instance == USART3)
	 {
			static bool in_frame_receiving = false;  // 唯一标识：in_frame_receiving
    
    if(received_byte == ARM_FRAME_HEADER)  // 检测到帧头
    {
        rx_buffer_index = 0;  // 清空缓冲区索引
        in_frame_receiving = true;  // 进入数据帧接收状态
        uart_rx_buffer[rx_buffer_index++] = received_byte;  // 存储帧头
    }
    else if(in_frame_receiving)
    {
        if(received_byte == ARM_FRAME_FOOTER)  // 检测到帧尾
        {   
            uart_rx_buffer[rx_buffer_index++] = received_byte;  // 存储帧尾
            uart_rx_buffer[rx_buffer_index] = '\0';    // 添加字符串结束符
            
            // 跳过帧头和帧尾，处理命令数据
            if(rx_buffer_index > 2) {
                process_arm_command((char*)uart_rx_buffer+1, rx_buffer_index-2);
            }
            
            in_frame_receiving = false;  // 结束数据帧接收
            rx_buffer_index = 0;  // 重置索引
        }
        else if(rx_buffer_index < BUFFER_SIZE-1)
        {
            uart_rx_buffer[rx_buffer_index++] = received_byte;  // 存储数据
        }
        else
        {
            // 缓冲区溢出，丢弃当前帧
            in_frame_receiving = false;
            rx_buffer_index = 0;
        }
    }
    HAL_UART_Receive_IT(&huart3, &received_byte, 1);
	}
}


/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
